/// ------------------------------------------------------------------------------------------------------------------------------------
///
/// MIT License
///
/// Permission is hereby granted, free of charge, to any person obtaining a copy
/// of this software and associated documentation files (the "Software"), to deal
/// in the Software without restriction, including without limitation the rights
/// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
/// copies of the Software, and to permit persons to whom the Software is
/// furnished to do so, subject to the following conditions:
///
/// The above copyright notice and this permission notice shall be included in all
/// copies or substantial portions of the Software.
///
/// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
/// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
/// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
/// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
/// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
/// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
/// SOFTWARE.
///
/// Copyright (c) 2024 ycz. All rights reserved.
///
/// Created by ycz on 24-7-15.
///
/// @file  y_alg.c
///
/// @brief
///     一种应用于嵌入式的简单算法工具
///
/// ------------------------------------------------------------------------------------------------------------------------------------



/// ------------------------------------------------------------------------------------------------------------------------------------
/// 头文件
/// ------------------------------------------------------------------------------------------------------------------------------------

#include "y_alg.h"

#include <math.h>
#include <string.h>
#include "y_sa.h"



/// ------------------------------------------------------------------------------------------------------------------------------------
/// 宏定义
/// ------------------------------------------------------------------------------------------------------------------------------------

#ifdef _Y_LL_H
#    define Y_MALLOC y_ll_malloc
#    define Y_FREE   y_ll_free
#else
#    define Y_MALLOC malloc
#    define Y_FREE   free
#endif



/// ------------------------------------------------------------------------------------------------------------------------------------
/// 公有函数
/// ------------------------------------------------------------------------------------------------------------------------------------

/// @brief   打印 y_alg 版本信息
void y_alg_print_version() {
    YLOG_VERSION("y_alg", Y_ALG_MAJOR, Y_ALG_MINOR, Y_ALG_PATCH);
}



/// ------------------------------------------------------------------------------------------------------------------------------------
/// 单精度 公有函数
/// ------------------------------------------------------------------------------------------------------------------------------------

/// @brief   截断小数
/// @param   [in] value                    数值
/// @param   [in] num                      截断小数个数 0=保留整数 num<10
/// @return  截断后的值
float y_alg_trunc_f(float value, uint8_t num) {

    if (num > 10) {
        num = 10;
    }

    float multiplier = 1;
    for (int i = 0; i < num; ++i) {
        multiplier *= 10;
    }

    float tmp = ((long long) (value * multiplier)) / multiplier;
    return tmp;
}

/// @brief   求最大值数组下标
/// @param   [in] arr                        数组指针
/// @param   [in] size                       数组大小
/// @return  最大值数组下标
uint16_t y_alg_get_max_index_f(float *arr, uint16_t size) {

    YLOGA_FALSE(arr);
    YLOGA_FALSE(size);

    uint16_t index = 0;
    float    tmp   = arr[0];
    for (int i = 1; i < size; ++i) {
        if (tmp < arr[i]) {
            tmp   = arr[i];
            index = i;
        }
    }
    return index;
}

/// @brief   求最小值数组下标
/// @param   [in] arr                        数组指针
/// @param   [in] size                       数组大小
/// @return  最小值数组下标
uint16_t y_alg_get_min_index_f(float *arr, uint16_t size) {

    YLOGA_FALSE(arr);
    YLOGA_FALSE(size);

    uint16_t index = 0;
    float    tmp   = arr[0];
    for (int i = 1; i < size; ++i) {
        if (tmp > arr[i]) {
            tmp   = arr[i];
            index = i;
        }
    }
    return index;
}

/// @brief   求最大值
/// @param   [in] arr                        数组指针
/// @param   [in] size                       数组大小
/// @return  最大值
float y_alg_get_max_f(float *arr, uint16_t size) {

    YLOGA_FALSE(arr);
    YLOGA_FALSE(size);

    float tmp = arr[0];
    for (int i = 1; i < size; ++i) {
        if (tmp < arr[i]) {
            tmp = arr[i];
        }
    }
    return tmp;
}

/// @brief   求最小值
/// @param   [in] arr                        数组指针
/// @param   [in] size                       数组大小
/// @return  最小值
float y_alg_get_min_f(float *arr, uint16_t size) {

    YLOGA_FALSE(arr);
    YLOGA_FALSE(size);

    float tmp = arr[0];
    for (int i = 1; i < size; ++i) {
        if (tmp > arr[i]) {
            tmp = arr[i];
        }
    }
    return tmp;
}

/// @brief   求和
/// @param   [in] arr                        数组指针
/// @param   [in] size                       数组大小
/// @return  和
float y_alg_get_sum_f(float *arr, uint16_t size) {

    YLOGA_FALSE(arr);
    YLOGA_FALSE(size);

    float tmp = arr[0];
    for (int i = 1; i < size; ++i) {
        tmp += arr[i];
    }
    return tmp;
}

/// @brief   求中位值
/// @details size 一般取奇数
/// @param   [in] arr                        数组指针
/// @param   [in] size                       数组大小
/// @return  中位值
float y_alg_get_mid_f(float *arr, uint16_t size) {

    YLOGA_FALSE(arr);
    YLOGA_FALSE(size);

    float tmp_arr[size];
    memcpy(tmp_arr, arr, sizeof(float) * size);

    // 冒泡排序 由小到大
    for (int i = 0; i < size - 1; i++) {
        for (int j = 0; j < size - 1 - i; j++) {
            if (tmp_arr[j] > tmp_arr[j + 1]) {
                float tmp      = tmp_arr[j];
                tmp_arr[j]     = tmp_arr[j + 1];
                tmp_arr[j + 1] = tmp;
            }
        }
    }
    return tmp_arr[(size - 1) / 2];
}

/// @brief   求平均值
/// @param   [in] arr                        数组指针
/// @param   [in] size                       数组大小
/// @return  平均值
float y_alg_get_avg_f(float *arr, uint16_t size) {

    YLOGA_FALSE(arr);
    YLOGA_FALSE(size);

    float tmp = arr[0];
    for (int i = 1; i < size; ++i) {
        tmp += arr[i];
    }
    return tmp / (float) size;
}

/// @brief   求方差
/// @param   [in] arr                        数组指针
/// @param   [in] size                       数组大小
/// @return  方差
float y_alg_get_var_f(float *arr, uint16_t size) {

    YLOGA_FALSE(arr);
    YLOGA_FALSE(size);

    float tmp = arr[0];
    for (int i = 1; i < size; ++i) {
        tmp += arr[i];
    }
    tmp       = tmp / (float) size;  // 求平均值

    float var = 0;
    for (int i = 0; i < size; ++i) {
        var += (tmp - arr[i]) * (tmp - arr[i]);
    }
    return var / (float) size;
}

/// @brief   求标准差
/// @param   [in] arr                        数组指针
/// @param   [in] size                       数组大小
/// @return  标准差
float y_alg_get_sd_f(float *arr, uint16_t size) {

    YLOGA_FALSE(arr);
    YLOGA_FALSE(size);

    float tmp = arr[0];
    for (int i = 1; i < size; ++i) {
        tmp += arr[i];
    }
    tmp       = tmp / (float) size;  // 求平均值

    float var = 0;
    for (int i = 0; i < size; ++i) {
        var += (tmp - arr[i]) * (tmp - arr[i]);
    }
    return sqrtf(var / (float) size);
}

/// @brief   求去掉最值的平均值
/// @param   [in] arr                        数组指针
/// @param   [in] size                       数组大小
/// @return  去掉最值的平均值
float y_alg_get_trimmean_f(float *arr, uint16_t size) {

    YLOGA_FALSE(arr);
    YLOGA_FALSE(size > 2);

    float min = arr[0];
    float max = arr[0];
    float tmp = arr[0];
    for (int i = 1; i < size; ++i) {
        if (min > arr[i]) {
            min = arr[i];
        }
        if (max < arr[i]) {
            max = arr[i];
        }
        tmp += arr[i];
    }
    return (tmp - max - min) / (float) (size - 2);
}

/// @brief   求加权平均值
/// @param   [in] arr                        数组指针
/// @param   [in] size                       数组大小
/// @param   [in] weighted_arr               加权指针
/// @return  加权平均值
float y_alg_get_weighted_avg_f(float *arr, uint16_t size, float *weighted_arr) {

    YLOGA_FALSE(arr);
    YLOGA_FALSE(size);
    YLOGA_FALSE(weighted_arr);

    float tmp          = 0;
    float weighted_sum = 0;
    for (int i = 0; i < size; i++) {
        tmp += arr[i] * weighted_arr[i];
        weighted_sum += weighted_arr[i];
    }
    tmp /= weighted_sum;
    return tmp;
}

/// @brief   限幅滤波
/// @details 本次采样值与上次滤波结果之差 < A 则本次采样值有效
/// @details 本次采样值与上次滤波结果之差 > A 则本次采样值无效 放弃本次值 采用上次滤波值
/// @details 优点: 能有效克服因偶然因素引起的脉冲干扰
/// @details 缺点: 无法抑制那种周期性的干扰 且平滑度差
/// @param   [in] now_value                当前值
/// @param   [in] last_value               上一次的值
/// @param   [in] threshold                阈值
/// @return  滤波后的值
float y_alg_filter_limiting_f(float now_value, float last_value, float threshold) {

    if ((now_value - last_value > threshold) || (last_value - now_value > threshold)) {
        return last_value;
    }
    return now_value;
}

/// @brief   一阶低通滤波
/// @details a * 本次采样值+（1-a）*上次滤波结果  去掉低频噪声
/// @details 优点: 对周期性干扰具有良好的抑制作用 适用于波动频率较高的场合
/// @details 缺点: 相位滞后，灵敏度低 滞后程度取决于a值大小 不能消除滤波频率高于采样频率的1/2的干扰信号
/// @param   [in] now_value                当前值
/// @param   [in] last_value               上一次的值
/// @param   [in] a                        滤波系数 设滤波时间为t，采样频率为F则a＝1/tF 越小滤波效果越好，但是达到稳定值的速度也越慢
/// @return  滤波后的值
float y_alg_filter_first_order_low_f(float now_value, float last_value, float a) {
    return (a * now_value) + ((1 - a) * last_value);
}

/// @brief   一阶高通滤波
/// @details (a-1) * 本次采样值+ a*上次滤波结果  去掉高频噪声
/// @details 优点: 对周期性干扰具有良好的抑制作用 适用于波动频率较高的场合
/// @details 缺点: 相位滞后，灵敏度低 滞后程度取决于a值大小 不能消除滤波频率高于采样频率的1/2的干扰信号
/// @param   [in] now_value                当前值
/// @param   [in] last_value               上一次的值
/// @param   [in] a                        滤波系数 设滤波时间为t，采样频率为F则a＝1/tF 越小滤波效果越好，但是达到稳定值的速度也越慢
/// @return  滤波后的值
float y_alg_filter_first_order_high_f(float now_value, float last_value, float a) {
    return ((1 - a) * now_value) + (a * last_value);
}

/// @brief   一阶互补滤波
/// @details 高通滤波和低通滤波一起来增加滤波得准确性  a<1
/// @param   [in] low_value                低通滤波结果
/// @param   [in] high_value               高通滤波结果
/// @param   [in] a                        低通滤波补偿权重
/// @return  滤波后的值
float y_alg_filter_first_order_complementary_f(float low_value, float high_value, float a) {
    return (low_value * a) + (high_value * (1 - a));
}

/// @brief   卡尔曼滤波
/// @param   [in] kalman                   卡尔曼滤波参数
/// @param   [in] now_value                当前值
/// @return  滤波后的值
float y_alg_filter_kalman_f(ALG_KALMAN_F_st *kalman, float now_value) {

    YLOGA_FALSE(kalman);

    kalman->now_p  = kalman->last_p + kalman->q;                            // k时系统估算协方差 = k-1时的系统协方差 + 过程噪声协方差
    kalman->kg     = kalman->now_p / (kalman->now_p + kalman->r);           // 卡尔曼增益 = k时系统估算协方差 / （k时系统估算协方差 + 观测噪声协方差）
    kalman->out    = kalman->out + kalman->kg * (now_value - kalman->out);  // k时状态变量的最优值 = 状态变量的预测值 + 卡尔曼增益 * （测量值 - 状态变量的预测值）
    kalman->last_p = (1 - kalman->kg) * kalman->now_p;                      // 本次的系统协方差付给 param->last_p 威下一次运算准备。

    return kalman->out;
}

/// @brief   IIR滤波  无限长冲激响应响应滤波器
/// @param   [in] iir                      IIR 滤波参数
/// @param   [in] now_value                当前值
/// @param   [in] fs                       采样频率
/// @param   [in] fc                       截止频率
/// @return  滤波后的值
float y_alg_filter_iir_f(ALG_IIR_F_st *iir, float now_value, uint16_t fs, float fc) {

    YLOGA_FALSE(iir);
    YLOGA_FALSE(fs);  // fs 不能为0
    YLOGA_FALSE(fc);  // fc 不能为0

    if (iir->a[0] == 0) {
        float fr  = (float) fs / fc;
        float omg = y_alg_trunc_f(tanf(Y_PI / fr), 6);
        float c   = y_alg_trunc_f(1 + (2 * cosf(Y_PI / 4) * omg) + (omg * omg), 6);
        iir->b[0] = omg * omg / c;
        iir->b[1] = 2 * omg * omg / c;
        iir->b[2] = iir->b[0];
        iir->a[0] = (2 * omg * omg - 2) / c;
        iir->a[1] = y_alg_trunc_f((1 - 2 * cosf(Y_PI / 4) * omg + omg * omg) / c, 6);
    }

    float d0    = now_value - (iir->a[0] * iir->arr[0]) - (iir->a[1] * iir->arr[1]);
    float y0    = (iir->b[0] * d0) + (iir->b[1] * iir->arr[0]) + (iir->b[2] * iir->arr[1]);

    iir->arr[1] = y_alg_trunc_f(iir->arr[0], 3);
    iir->arr[0] = y_alg_trunc_f(d0, 3);

    return y_alg_trunc_f(y0, 3);  // 保留 3 位小数
}

/// @brief   创建窗口
/// @param   [in] size                     窗口大小
/// @param   [in] slide                    窗口滑动大小 窗口数据满后 有新数据覆盖时 向前滑动的大小
/// @return  窗口句柄
ALG_WINDOW_F_st *y_alg_window_create_f(uint16_t size, uint16_t slide) {

    YLOGA_NULL(size);
    YLOGA_NULL(size >= slide);

    ALG_WINDOW_F_st *win = Y_MALLOC(sizeof(ALG_WINDOW_F_st));
    YLOGA_NULL(win);
    memset(win, 0, sizeof(ALG_WINDOW_F_st));
    win->size  = size;
    win->slide = slide;
    win->arr   = Y_MALLOC(sizeof(float) * size);
    if (win->arr == NULL) {
        Y_FREE(win);
        return NULL;
    }
    memset(win->arr, 0, sizeof(float) * size);

    return win;
}

/// @brief   销毁窗口
/// @param   [in] win                      窗口句柄
/// @retval  true                          成功
/// @retval  false                         失败
bool y_alg_window_destroy_f(ALG_WINDOW_F_st *win) {

    YLOGA_FALSE(win);

    Y_FREE(win->arr);
    Y_FREE(win);

    return true;
}

/// @brief   窗口数据是否为空
/// @param   [in] win                      窗口句柄
/// @retval  true                          成功
/// @retval  false                         失败
bool y_alg_window_is_empty_f(ALG_WINDOW_F_st *win) {
    YLOGA_FALSE(win);
    return (win->index == 0) ? true : false;
}

/// @brief   窗口数据是否为满
/// @param   [in] win                      窗口句柄
/// @retval  true                          成功
/// @retval  false                         失败
bool y_alg_window_is_full_f(ALG_WINDOW_F_st *win) {
    YLOGA_FALSE(win);
    return (win->index == win->size) ? true : false;
}

/// @brief   往窗口添加值
/// @param   [in] win                      窗口句柄
/// @param   [in] value                    值
/// @retval  true                          成功
/// @retval  false                         失败
bool y_alg_window_add_f(ALG_WINDOW_F_st *win, float value) {

    YLOGA_FALSE(win);

    // 判断数组是否已满
    if (win->index == win->size) {
        // 判断滑动大小
        if (win->size == win->slide || win->size == 1) {  // 每次滑动整个窗口 或窗口只有一个大小
            memset(win->arr, 0, sizeof(win->arr[0]) * win->size);
            win->index = 0;
        } else {
            for (int i = 0; i < win->size - win->slide; ++i) {
                win->arr[i] = win->arr[i + win->slide];  // 窗口滑动前移
            }
            win->index -= win->slide;
            memset(&win->arr[win->index], 0, sizeof(win->arr[0]) * win->slide);
        }
    }

    // 添加新数据
    win->arr[win->index++] = value;

    return true;
}

/// @brief   往窗口添加数组
/// @param   [in] win                      窗口句柄
/// @param   [in] arr                      数组指针
/// @param   [in] size                     数组大小
/// @retval  true                          成功
/// @retval  false                         失败
bool y_alg_window_add_arr_f(ALG_WINDOW_F_st *win, float *arr, uint16_t size) {

    YLOGA_FALSE(arr);
    YLOGA_FALSE(size);

    for (int i = 0; i < size; ++i) {
        if (y_alg_window_add_f(win, arr[i]) == false) {
            return false;
        }
    }
    return true;
}

/// @brief   从窗口删除值 从后往前删除
/// @param   [in] win                      窗口句柄
/// @retval  true                          成功
/// @retval  false                         失败
bool y_alg_window_del_f(ALG_WINDOW_F_st *win) {

    YLOGA_FALSE(win);

    if (win->index != 0) {
        win->index--;
        win->arr[win->index] = 0;
    }

    return true;
}

/// @brief   清空窗口值
/// @param   [in] win                      窗口句柄
/// @retval  true                          成功
/// @retval  false                         失败
bool y_alg_window_clear_f(ALG_WINDOW_F_st *win) {

    YLOGA_FALSE(win);

    memset(win->arr, 0, sizeof(win->arr[0]) * win->size);
    win->index = 0;

    return true;
}

/// @brief   打印窗口值
/// @param   [in] win                      窗口句柄
/// @retval  true                          成功
/// @retval  false                         失败
void y_alg_window_print_f(ALG_WINDOW_F_st *win) {

    YLOGA(win);
    printf("\r\n");
    for (int i = 0; i < win->size; ++i) {
        printf("%.6f  ", win->arr[i]);
    }
    printf("\r\n\r\n");
}



/// ------------------------------------------------------------------------------------------------------------------------------------
/// 双精度 公有函数
/// ------------------------------------------------------------------------------------------------------------------------------------

/// @brief   截断小数
/// @param   [in] value                    数值
/// @param   [in] num                      截断小数个数 0=保留整数 num<10
/// @return  截断后的值
double y_alg_trunc_d(double value, uint8_t num) {

    if (num > 10) {
        num = 10;
    }

    double multiplier = 1;
    for (int i = 0; i < num; ++i) {
        multiplier *= 10;
    }

    double tmp = ((long long) (value * multiplier)) / multiplier;
    return tmp;
}

/// @brief   求最大值数组下标
/// @param   [in] arr                        数组指针
/// @param   [in] size                       数组大小
/// @return  最大值数组下标
uint16_t y_alg_get_max_index_d(double *arr, uint16_t size) {

    YLOGA_FALSE(arr);
    YLOGA_FALSE(size);

    uint16_t index = 0;
    double   tmp   = arr[0];
    for (int i = 1; i < size; ++i) {
        if (tmp < arr[i]) {
            tmp   = arr[i];
            index = i;
        }
    }
    return index;
}

/// @brief   求最小值数组下标
/// @param   [in] arr                        数组指针
/// @param   [in] size                       数组大小
/// @return  最小值数组下标
uint16_t y_alg_get_min_index_d(double *arr, uint16_t size) {

    YLOGA_FALSE(arr);
    YLOGA_FALSE(size);

    uint16_t index = 0;
    double   tmp   = arr[0];
    for (int i = 1; i < size; ++i) {
        if (tmp > arr[i]) {
            tmp   = arr[i];
            index = i;
        }
    }
    return index;
}

/// @brief   求最大值
/// @param   [in] arr                        数组指针
/// @param   [in] size                       数组大小
/// @return  最大值
double y_alg_get_max_d(double *arr, uint16_t size) {

    YLOGA_FALSE(arr);
    YLOGA_FALSE(size);

    double tmp = arr[0];
    for (int i = 1; i < size; ++i) {
        if (tmp < arr[i]) {
            tmp = arr[i];
        }
    }
    return tmp;
}

/// @brief   求最小值
/// @param   [in] arr                        数组指针
/// @param   [in] size                       数组大小
/// @return  最小值
double y_alg_get_min_d(double *arr, uint16_t size) {

    YLOGA_FALSE(arr);
    YLOGA_FALSE(size);

    double tmp = arr[0];
    for (int i = 1; i < size; ++i) {
        if (tmp > arr[i]) {
            tmp = arr[i];
        }
    }
    return tmp;
}

/// @brief   求和
/// @param   [in] arr                        数组指针
/// @param   [in] size                       数组大小
/// @return  和
double y_alg_get_sum_d(double *arr, uint16_t size) {

    YLOGA_FALSE(arr);
    YLOGA_FALSE(size);

    double tmp = arr[0];
    for (int i = 1; i < size; ++i) {
        tmp += arr[i];
    }
    return tmp;
}

/// @brief   求中位值
/// @details size 一般取奇数
/// @param   [in] arr                        数组指针
/// @param   [in] size                       数组大小
/// @return  中位值
double y_alg_get_mid_d(double *arr, uint16_t size) {

    YLOGA_FALSE(arr);
    YLOGA_FALSE(size);

    double tmp_arr[size];
    memcpy(tmp_arr, arr, sizeof(double) * size);

    // 冒泡排序 由小到大
    for (int i = 0; i < size - 1; i++) {
        for (int j = 0; j < size - 1 - i; j++) {
            if (tmp_arr[j] > tmp_arr[j + 1]) {
                double tmp     = tmp_arr[j];
                tmp_arr[j]     = tmp_arr[j + 1];
                tmp_arr[j + 1] = tmp;
            }
        }
    }
    return tmp_arr[(size - 1) / 2];
}

/// @brief   求平均值
/// @param   [in] arr                        数组指针
/// @param   [in] size                       数组大小
/// @return  平均值
double y_alg_get_avg_d(double *arr, uint16_t size) {

    YLOGA_FALSE(arr);
    YLOGA_FALSE(size);

    double tmp = arr[0];
    for (int i = 1; i < size; ++i) {
        tmp += arr[i];
    }
    return tmp / size;
}

/// @brief   求方差
/// @param   [in] arr                        数组指针
/// @param   [in] size                       数组大小
/// @return  方差
double y_alg_get_var_d(double *arr, uint16_t size) {

    YLOGA_FALSE(arr);
    YLOGA_FALSE(size);

    double tmp = arr[0];
    for (int i = 1; i < size; ++i) {
        tmp += arr[i];
    }
    tmp        = tmp / size;  // 求平均值

    double var = 0;
    for (int i = 0; i < size; ++i) {
        var += (tmp - arr[i]) * (tmp - arr[i]);
    }
    return var / size;
}

/// @brief   求标准差
/// @param   [in] arr                        数组指针
/// @param   [in] size                       数组大小
/// @return  标准差
double y_alg_get_sd_d(double *arr, uint16_t size) {

    YLOGA_FALSE(arr);
    YLOGA_FALSE(size);

    double tmp = arr[0];
    for (int i = 1; i < size; ++i) {
        tmp += arr[i];
    }
    tmp        = tmp / size;  // 求平均值

    double var = 0;
    for (int i = 0; i < size; ++i) {
        var += (tmp - arr[i]) * (tmp - arr[i]);
    }
    return sqrt(var / size);
}

/// @brief   求去掉最值的平均值
/// @param   [in] arr                        数组指针
/// @param   [in] size                       数组大小
/// @return  去掉最值的平均值
double y_alg_get_trimmean_d(double *arr, uint16_t size) {

    YLOGA_FALSE(arr);
    YLOGA_FALSE(size > 2);

    double min = arr[0];
    double max = arr[0];
    double tmp = arr[0];
    for (int i = 1; i < size; ++i) {
        if (min > arr[i]) {
            min = arr[i];
        }
        if (max < arr[i]) {
            max = arr[i];
        }
        tmp += arr[i];
    }
    return (tmp - max - min) / (size - 2);
}

/// @brief   求加权平均值
/// @param   [in] arr                        数组指针
/// @param   [in] size                       数组大小
/// @param   [in] weighted_arr               加权指针
/// @return  加权平均值
double y_alg_get_weighted_avg_d(double *arr, uint16_t size, double *weighted_arr) {

    YLOGA_FALSE(arr);
    YLOGA_FALSE(size);
    YLOGA_FALSE(weighted_arr);

    double tmp          = 0;
    double weighted_sum = 0;
    for (int i = 0; i < size; i++) {
        tmp += arr[i] * weighted_arr[i];
        weighted_sum += weighted_arr[i];
    }
    tmp /= weighted_sum;
    return tmp;
}

/// @brief   限幅滤波
/// @details 本次采样值与上次滤波结果之差 < A 则本次采样值有效
/// @details 本次采样值与上次滤波结果之差 > A 则本次采样值无效 放弃本次值 采用上次滤波值
/// @details 优点: 能有效克服因偶然因素引起的脉冲干扰
/// @details 缺点: 无法抑制那种周期性的干扰 且平滑度差
/// @param   [in] now_value                当前值
/// @param   [in] last_value               上一次的值
/// @param   [in] threshold                阈值
/// @return  滤波后的值
double y_alg_filter_limiting_d(double now_value, double last_value, double threshold) {

    if ((now_value - last_value > threshold) || (last_value - now_value > threshold)) {
        return last_value;
    }
    return now_value;
}

/// @brief   一阶低通滤波
/// @details a * 本次采样值+（1-a）*上次滤波结果  去掉低频噪声
/// @details 优点: 对周期性干扰具有良好的抑制作用 适用于波动频率较高的场合
/// @details 缺点: 相位滞后，灵敏度低 滞后程度取决于a值大小 不能消除滤波频率高于采样频率的1/2的干扰信号
/// @param   [in] now_value                当前值
/// @param   [in] last_value               上一次的值
/// @param   [in] a                        滤波系数 设滤波时间为t，采样频率为F则a＝1/tF 越小滤波效果越好，但是达到稳定值的速度也越慢
/// @return  滤波后的值
double y_alg_filter_first_order_low_d(double now_value, double last_value, double a) {
    return (a * now_value) + ((1 - a) * last_value);
}

/// @brief   一阶高通滤波
/// @details (a-1) * 本次采样值+ a*上次滤波结果  去掉高频噪声
/// @details 优点: 对周期性干扰具有良好的抑制作用 适用于波动频率较高的场合
/// @details 缺点: 相位滞后，灵敏度低 滞后程度取决于a值大小 不能消除滤波频率高于采样频率的1/2的干扰信号
/// @param   [in] now_value                当前值
/// @param   [in] last_value               上一次的值
/// @param   [in] a                        滤波系数 设滤波时间为t，采样频率为F则a＝1/tF 越小滤波效果越好，但是达到稳定值的速度也越慢
/// @return  滤波后的值
double y_alg_filter_first_order_high_d(double now_value, double last_value, double a) {
    return ((1 - a) * now_value) + (a * last_value);
}

/// @brief   一阶互补滤波
/// @details 高通滤波和低通滤波一起来增加滤波得准确性  a<1
/// @param   [in] low_value                低通滤波结果
/// @param   [in] high_value               高通滤波结果
/// @param   [in] a                        低通滤波补偿权重
/// @return  滤波后的值
double y_alg_filter_first_order_complementary_d(double low_value, double high_value, double a) {
    return (low_value * a) + (high_value * (1 - a));
}

/// @brief   卡尔曼滤波
/// @param   [in] kalman                   卡尔曼滤波参数
/// @param   [in] now_value                当前值
/// @return  滤波后的值
double y_alg_filter_kalman_d(ALG_KALMAN_D_st *kalman, double now_value) {

    YLOGA_FALSE(kalman);

    kalman->now_p  = kalman->last_p + kalman->q;                            // k时系统估算协方差 = k-1时的系统协方差 + 过程噪声协方差
    kalman->kg     = kalman->now_p / (kalman->now_p + kalman->r);           // 卡尔曼增益 = k时系统估算协方差 / （k时系统估算协方差 + 观测噪声协方差）
    kalman->out    = kalman->out + kalman->kg * (now_value - kalman->out);  // k时状态变量的最优值 = 状态变量的预测值 + 卡尔曼增益 * （测量值 - 状态变量的预测值）
    kalman->last_p = (1 - kalman->kg) * kalman->now_p;                      // 本次的系统协方差付给 param->last_p 威下一次运算准备。

    return kalman->out;
}

/// @brief   IIR滤波  无限长冲激响应响应滤波器
/// @param   [in] iir                      IIR 滤波参数
/// @param   [in] now_value                当前值
/// @param   [in] fs                       采样频率
/// @param   [in] fc                       截止频率
/// @return  滤波后的值
double y_alg_filter_iir_d(ALG_IIR_D_st *iir, double now_value, uint16_t fs, double fc) {

    YLOGA_FALSE(iir);
    YLOGA_FALSE(fs);  // fs 不能为0
    YLOGA_FALSE(fc);  // fc 不能为0

    if (iir->a[0] == 0) {
        double fr  = fs / fc;
        double omg = y_alg_trunc_d(tan(Y_PI / fr), 6);
        double c   = y_alg_trunc_d(1 + (2 * cos(Y_PI / 4) * omg) + (omg * omg), 6);
        iir->b[0]  = omg * omg / c;
        iir->b[1]  = 2 * omg * omg / c;
        iir->b[2]  = iir->b[0];
        iir->a[0]  = (2 * omg * omg - 2) / c;
        iir->a[1]  = y_alg_trunc_d((1 - 2 * cos(Y_PI / 4) * omg + omg * omg) / c, 6);
    }

    double d0   = now_value - (iir->a[0] * iir->arr[0]) - (iir->a[1] * iir->arr[1]);
    double y0   = (iir->b[0] * d0) + (iir->b[1] * iir->arr[0]) + (iir->b[2] * iir->arr[1]);

    iir->arr[1] = y_alg_trunc_d(iir->arr[0], 3);
    iir->arr[0] = y_alg_trunc_d(d0, 3);

    return y_alg_trunc_d(y0, 3);  // 保留 3 位小数
}

/// @brief   创建窗口
/// @param   [in] size                     窗口大小
/// @param   [in] slide                    窗口滑动大小 窗口数据满后 有新数据覆盖时 向前滑动的大小
/// @return  窗口句柄
ALG_WINDOW_D_st *y_alg_window_create_d(uint16_t size, uint16_t slide) {

    YLOGA_NULL(size);
    YLOGA_NULL(size >= slide);

    ALG_WINDOW_D_st *win = Y_MALLOC(sizeof(ALG_WINDOW_D_st));
    YLOGA_NULL(win);
    memset(win, 0, sizeof(ALG_WINDOW_D_st));
    win->size  = size;
    win->slide = slide;
    win->arr   = Y_MALLOC(sizeof(double) * size);
    if (win->arr == NULL) {
        Y_FREE(win);
        return NULL;
    }
    memset(win->arr, 0, sizeof(double) * size);

    return win;
}

/// @brief   销毁窗口
/// @param   [in] win                      窗口句柄
/// @retval  true                          成功
/// @retval  false                         失败
bool y_alg_window_destroy_d(ALG_WINDOW_D_st *win) {

    YLOGA_FALSE(win);

    Y_FREE(win->arr);
    Y_FREE(win);

    return true;
}

/// @brief   窗口数据是否为空
/// @param   [in] win                      窗口句柄
/// @retval  true                          成功
/// @retval  false                         失败
bool y_alg_window_is_empty_d(ALG_WINDOW_D_st *win) {
    YLOGA_FALSE(win);
    return (win->index == 0) ? true : false;
}

/// @brief   窗口数据是否为满
/// @param   [in] win                      窗口句柄
/// @retval  true                          成功
/// @retval  false                         失败
bool y_alg_window_is_full_d(ALG_WINDOW_D_st *win) {
    YLOGA_FALSE(win);
    return (win->index == win->size) ? true : false;
}

/// @brief   往窗口添加值
/// @param   [in] win                      窗口句柄
/// @param   [in] value                    值
/// @retval  true                          成功
/// @retval  false                         失败
bool y_alg_window_add_d(ALG_WINDOW_D_st *win, double value) {

    YLOGA_FALSE(win);

    // 判断数组是否已满
    if (win->index == win->size) {
        // 判断滑动大小
        if (win->size == win->slide || win->size == 1) {  // 每次滑动整个窗口 或窗口只有一个大小
            memset(win->arr, 0, sizeof(win->arr[0]) * win->size);
            win->index = 0;
        } else {
            for (int i = 0; i < win->size - win->slide; ++i) {
                win->arr[i] = win->arr[i + win->slide];  // 窗口滑动前移
            }
            win->index -= win->slide;
            memset(&win->arr[win->index], 0, sizeof(win->arr[0]) * win->slide);
        }
    }

    // 添加新数据
    win->arr[win->index++] = value;

    return true;
}

/// @brief   往窗口添加数组
/// @param   [in] win                      窗口句柄
/// @param   [in] arr                      数组指针
/// @param   [in] size                     数组大小
/// @retval  true                          成功
/// @retval  false                         失败
bool y_alg_window_add_arr_d(ALG_WINDOW_D_st *win, double *arr, uint16_t size) {

    YLOGA_FALSE(arr);
    YLOGA_FALSE(size);

    for (int i = 0; i < size; ++i) {
        if (y_alg_window_add_d(win, arr[i]) == false) {
            return false;
        }
    }
    return true;
}

/// @brief   从窗口删除值 从后往前删除
/// @param   [in] win                      窗口句柄
/// @retval  true                          成功
/// @retval  false                         失败
bool y_alg_window_del_d(ALG_WINDOW_D_st *win) {

    YLOGA_FALSE(win);

    if (win->index != 0) {
        win->index--;
        win->arr[win->index] = 0;
    }

    return true;
}

/// @brief   清空窗口值
/// @param   [in] win                      窗口句柄
/// @retval  true                          成功
/// @retval  false                         失败
bool y_alg_window_clear_d(ALG_WINDOW_D_st *win) {

    YLOGA_FALSE(win);

    memset(win->arr, 0, sizeof(win->arr[0]) * win->size);
    win->index = 0;

    return true;
}

/// @brief   打印窗口值
/// @param   [in] win                      窗口句柄
/// @retval  true                          成功
/// @retval  false                         失败
void y_alg_window_print_d(ALG_WINDOW_D_st *win) {

    YLOGA(win);
    printf("\r\n");
    for (int i = 0; i < win->size; ++i) {
        printf("%.6f  ", win->arr[i]);
    }
    printf("\r\n\r\n");
}
